#include <boost/algorithm/string.hpp>
#include <tf/transform_broadcaster.h>
#include <tf/transform_datatypes.h>

#include "JasonActionTurn.h"

#ifndef PI
 #define PI 3.14159265358979323846
#endif

#define Clamp(_min,_max,_val)   ((_min)>(_val)?(_min):((_max)<(_val)?(_max):(_val)))

CJasonActionTurn::CJasonActionTurn( ros::NodeHandle &rNode, E_Orientation eOrientation, const jason_msgs::action::ConstPtr& poActionMsg )
: CJasonAction()
, m_rNode( rNode )
, m_wwId( poActionMsg->action_id )
, m_strAction( poActionMsg->action )
, m_strAgent( poActionMsg->agent )
, m_bnHasInitialAngle( false )
, m_eOrientation( eOrientation )
{
    const std::string strDirection( boost::to_upper_copy( poActionMsg->parameters[0] ) );
    
    if ( strDirection == "LEFT" ) {

        m_oMsg.angular.z = 0.5;
    }
    else if ( strDirection == "RIGHT" ) {

        m_oMsg.angular.z = -0.5;
    }
    else {

      m_oMsg.angular.z = 0.0;
    }

    m_fAngle =  ( Clamp( 1, 180, boost::lexical_cast< float >( poActionMsg->parameters[1] ) ) );
}

void CJasonActionTurn::Start() {
    
    if ( !m_bnDone ) {
        
        m_oCmdvelPub = m_rNode.advertise< geometry_msgs::Twist >( "/fault_injector/cmd_vel"    , 10 );
        m_oEventPub  = m_rNode.advertise< jason_msgs::event    >( "/jason/event", 10 );
        m_oOdomSub   = m_rNode.subscribe< nav_msgs::Odometry   >( "/fault_injector/odom", 10, &CJasonActionTurn::ReceiveOdometry, this );
    }
}

void CJasonActionTurn::Step() {
    
    if ( m_bnHasInitialAngle && !m_bnDone ) {
    
        m_oCmdvelPub.publish( m_oMsg );
    }
}

void CJasonActionTurn::ReceiveOdometry( const nav_msgs::Odometry::ConstPtr& msg ) {

    if ( !m_bnHasInitialAngle ) {
        
        const double Z = msg.get()->pose.pose.orientation.z;
        
        m_fInitialAngle = ( Z >= 0.0 ? Z : 2.0 + Z ) * 180.0;
//        ROS_INFO("angulo inicial %f", m_fInitialAngle );
        
        m_bnHasInitialAngle = true;
        Step();
    }
    else {

        const double Z = msg.get()->pose.pose.orientation.z;
        
        const float fCurrAngle = ( Z >= 0.0 ? Z : 2.0 + Z ) * 180.0;
        float fDiff;
        
        fDiff = fabs( m_oMsg.angular.z < 0.0 ? m_fInitialAngle - fCurrAngle : fCurrAngle - m_fInitialAngle );
        if ( fDiff > 180.0 ) {

            fDiff = 360 - fDiff;
        }
        
//        ROS_INFO("Z = %f   angulo atual %f     dif %f", Z, fCurrAngle, fDiff );
        
        if ( fabs( fDiff - m_fAngle ) < 0.2 || fDiff > m_fAngle ) {
            
            m_bnDone = true;

            jason_msgs::event e;
            e.agent = m_strAgent;
            e.event_type = "ActionFeedback";
            e.parameters.push_back( m_strAction );
            e.parameters.push_back( boost::str( boost::format( "%d" ) % m_wwId ) );
            e.parameters.push_back( "Ok" );

            m_oEventPub.publish( e );

            m_oOdomSub.shutdown();
            
//            ROS_INFO("done.\n\n\n");
        }
        else if ( fabs( fDiff - m_fAngle ) < 3.0 ) {
            
            if ( m_oMsg.angular.z < 0.0 ) {
            
                m_oMsg.angular.z = -0.2;
            }
            else {
                
                m_oMsg.angular.z = 0.2;
            }
        }
        else if ( fabs( fDiff - m_fAngle ) < 5.0 ) {
            
            if ( m_oMsg.angular.z < 0.0 ) {
            
                m_oMsg.angular.z = -0.3;
            }
            else {
                
                m_oMsg.angular.z = 0.3;
            }
        }
    }
}

CJasonActionTurn::~CJasonActionTurn() {
    
}
